<HTML> 
<HEAD> 
	<TITLE>3rd Party ROBOTC Drivers</TITLE> 
<link href="tabs.css" rel="stylesheet" type="text/css"/> 
<link href="doxygen.css" rel="stylesheet" type="text/css"/> 
</HEAD> 
 
<BODY bgcolor="#ffffff" link="#000000" vlink="#000000"> 
<table width="100%" bgcolor="navy" cellspacing=0 cellpadding=1 border=0> 
<tr><td><table width="100%" bgcolor="#EEEEDD" cellspacing=0 cellpadding=3 border=0> 
<tr> 
    <td width="33%" align="left"> <img src="images/LOGO_NXT.gif" width=266 height=44 border="0" alt="Mindstorms"></td> 
    <td><b><font size="+3" color="navy">3rd Party ROBOTC Drivers</font></b></td> 
    <td align="right"> <img src="images/logo.png" width=44 height=44 border="0" alt="RobotC"></td>     
</tr> 
  <tr bgcolor="#cccc99" align="right"> 
          <td colspan=3> <font face="arial" size="-1"> [<a href=main.html>Home</a>] [<a target=_top href="https://sourceforge.net/projects/rdpartyrobotcdr/">Download</a>] [<a target=_top href="http://apps.sourceforge.net/mantisbt/rdpartyrobotcdr/my_view_page.php">Submit a bug/suggestion</a>]  [<a target=_top href="http://www.robotc.net/forums/">ROBOTC Forums</a>] [<a target=_top href="http://botbench.com">Blog</a>] [<a target=_top href="http://sourceforge.net/donate/index.php?group_id=257238">Support this project</a>]</font> </td> 
</tr> 
</table></td></tr> 
</table> 
<!-- Generated by Doxygen 1.7.2 -->
<div class="header">
  <div class="headertitle">
<h1>hitechnic-compass-test2.c</h1>  </div>
</div>
<div class="contents">
<div class="fragment"><pre class="fragment"><span class="preprocessor">#pragma config(Sensor, S1,     HTCOMPASS,           sensorI2CCustom)</span>
<span class="preprocessor"></span><span class="preprocessor">#pragma config(Motor,  motorB,          M_RIGHT,       tmotorNormal, PIDControl, reversed)</span>
<span class="preprocessor"></span><span class="preprocessor">#pragma config(Motor,  motorC,          M_LEFT,        tmotorNormal, PIDControl, reversed)</span>
<span class="preprocessor"></span><span class="comment">//*!!Code automatically generated by &#39;ROBOTC&#39; configuration wizard               !!*//</span>

<span class="comment">/*</span>
<span class="comment"> * $Id: hitechnic-compass-test2.c 133 2013-03-10 15:15:38Z xander $</span>
<span class="comment"> */</span>
<span class="comment"></span>
<span class="comment">/**</span>
<span class="comment"> * hitechnic-compass.h provides an API for the HiTechnic Compass Sensor.  This program</span>
<span class="comment"> * demonstrates how to use that API to calibrate the compass.</span>
<span class="comment"> *</span>
<span class="comment"> * The code here is made to work with the standard NXT Tribot.  You will need to edit</span>
<span class="comment"> * the WHEELDIST, WHEELSIZE and MOTORSPEED figures to make it work with your robot.</span>
<span class="comment"> *</span>
<span class="comment"> * Remeber that the robot shouldn&#39;t spin more than 360 degrees per 20 seconds.  Also</span>
<span class="comment"> * make sure it spins a bit more than 360, perhaps 1 and 1/4 or 1 and 1/2.</span>
<span class="comment"> *</span>
<span class="comment"> * Changelog:</span>
<span class="comment"> * - 0.1: Initial release</span>
<span class="comment"> * - 0.2: Added pulsating &quot;*&quot; to indicate progress</span>
<span class="comment"> * - 0.3: More comments</span>
<span class="comment"> *</span>
<span class="comment"> * License: You may use this code as you wish, provided you give credit where it&#39;s due.</span>
<span class="comment"> *</span>
<span class="comment"> * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.59 AND HIGHER. </span>
<span class="comment"></span>
<span class="comment"> * Xander Soldaat (xander_at_botbench.com)</span>
<span class="comment"> * 25 November 2009</span>
<span class="comment"> * version 0.3</span>
<span class="comment"> */</span>

<span class="preprocessor">#include &quot;<a class="code" href="hitechnic-compass_8h.html" title="HiTechnic Magnetic Compass Sensor Driver.">drivers/hitechnic-compass.h</a>&quot;</span>

<span class="comment">// These measurements are in milimeters.</span>
<span class="preprocessor">#define WHEELDIST 115  // distance between the wheels</span>
<span class="preprocessor"></span><span class="preprocessor">#define WHEELSIZE 56   // diameter of the wheels</span>
<span class="preprocessor"></span>
<span class="preprocessor">#define MOTORSPEED 4   // speed at which motors should turn</span>
<span class="preprocessor"></span>
<span class="comment">// Lets you know when 20 seconds is over, can help with setting up</span>
<span class="comment">// the initial timing and motor speed.</span>
task timeMe() {
  wait1Msec(20000);
  PlaySound(soundBeepBeep);
  <span class="keywordflow">while</span>(bSoundActive) EndTimeSlice();
}

<span class="comment">// Pulse a big &quot;*&quot; at the bottom of the screen to show that it&#39;s</span>
<span class="comment">// doing something.</span>
task showPulse() {
  <span class="keywordflow">while</span> (<span class="keyword">true</span>) {
                nxtDisplayCenteredBigTextLine(6, <span class="stringliteral">&quot; &quot;</span>);
                wait1Msec(400);
                nxtDisplayCenteredBigTextLine(6, <span class="stringliteral">&quot;*&quot;</span>);
                wait1Msec(400);
  }
}


<span class="comment">// Does some voodoo maths to calculate how many times the wheels should rotate</span>
<span class="comment">// to make the robot spin about 360 degrees.</span>
<span class="keywordtype">int</span> numRotations() {
  <span class="keywordflow">return</span> ((WHEELDIST * 3142) / 1000) / ((WHEELSIZE * 3142) / 1000);
}

<span class="comment">// Start the calibration and complain loudly if something goes wrong</span>
<span class="keywordtype">void</span> startCalibration() {
  <span class="keywordflow">if</span> (!<a name="a0"></a><a class="code" href="group__htmc.html#ga7ef44956e95df323ad2dd282f8fa2dd5">HTMCstartCal</a>(HTCOMPASS)) {
    eraseDisplay();
    nxtDisplayTextLine(1, <span class="stringliteral">&quot;ERROR: Couldn&#39;t&quot;</span>);
    nxtDisplayTextLine(2, <span class="stringliteral">&quot;calibrate sensor.&quot;</span>);
    nxtDisplayTextLine(4, <span class="stringliteral">&quot;Check connection&quot;</span>);
    nxtDisplayTextLine(5, <span class="stringliteral">&quot;and try again.&quot;</span>);
    PlaySound(soundException);
    <span class="keywordflow">while</span>(bSoundActive) EndTimeSlice();
    wait1Msec(5000);
    StopAllTasks();
  }
}

<span class="comment">// Stop the calibration and complain loudly if somethign goes wrong</span>
<span class="keywordtype">void</span> stopCalibration() {
  <span class="keywordflow">if</span> (!<a name="a1"></a><a class="code" href="group__htmc.html#ga1a5811a058b34bfc4c655a432be362c0">HTMCstopCal</a>(HTCOMPASS)) {
    eraseDisplay();
    nxtDisplayTextLine(1, <span class="stringliteral">&quot;ERROR: Calibration&quot;</span>);
    nxtDisplayTextLine(2, <span class="stringliteral">&quot;has failed.&quot;</span>);
    nxtDisplayTextLine(4, <span class="stringliteral">&quot;Check connection&quot;</span>);
    nxtDisplayTextLine(5, <span class="stringliteral">&quot;and try again.&quot;</span>);
    PlaySound(soundException);
    <span class="keywordflow">while</span>(bSoundActive) EndTimeSlice();
    wait1Msec(5000);
    StopAllTasks();
  } <span class="keywordflow">else</span> {
    nxtDisplayTextLine(1, <span class="stringliteral">&quot;SUCCESS: &quot;</span>);
    nxtDisplayTextLine(2, <span class="stringliteral">&quot;Calibr. done.&quot;</span>);
    PlaySound(soundUpwardTones);
    <span class="keywordflow">while</span>(bSoundActive) EndTimeSlice();
    wait1Msec(5000);
  }
}

task main () {
  bFloatDuringInactiveMotorPWM = <span class="keyword">true</span>;
  <span class="keywordtype">int</span> numDegrees = 0;
  nxtDisplayCenteredTextLine(0, <span class="stringliteral">&quot;HiTechnic&quot;</span>);
  nxtDisplayCenteredBigTextLine(1, <span class="stringliteral">&quot;Compass&quot;</span>);
  nxtDisplayCenteredTextLine(3, <span class="stringliteral">&quot;Test 2&quot;</span>);


  nMotorEncoder[M_RIGHT] = 0;
  nMotorEncoder[M_LEFT] = 0;
  <span class="comment">// This will make the robot spin about 1.5 times, depends on many factors, YYMV, etc</span>
  numDegrees = ((numRotations() * 3) / 2) * 450;
  StartTask(timeMe);
  startCalibration();
  nxtDisplayCenteredTextLine(5, <span class="stringliteral">&quot;Calibrating...&quot;</span>);
  StartTask(showPulse);
  motor[M_RIGHT] = MOTORSPEED;
  motor[M_LEFT] = -MOTORSPEED;

  <span class="keywordflow">while</span>(nMotorEncoder[M_RIGHT] &lt; numDegrees) wait1Msec(5);
  motor[M_LEFT] = 0;
  motor[M_RIGHT]= 0;
  stopCalibration();
  nxtDisplayCenteredTextLine(5, <span class="stringliteral">&quot;Calibration done&quot;</span>);
  wait1Msec(5000);
}

<span class="comment">/*</span>
<span class="comment"> * $Id: hitechnic-compass-test2.c 133 2013-03-10 15:15:38Z xander $</span>
<span class="comment"> */</span>
</pre></div> </div>
</div>
<hr class="footer"/><address class="footer"><small>Generated on Wed Mar 13 2013 19:56:42 for ROBOTC Drivers by&#160;
<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.7.2 </small></address>
</body>
</html>
